/*
恒速模式例子在STS3215中测试通过，舵机出厂速度单位V=0为停止状态
*/
#ifdef U8X8_HAVE_HW_SPI
#include <SPI.h>
#endif
#ifdef U8X8_HAVE_HW_I2C
#include <Wire.h>
#endif
#include <SCServo.h>

SMS_STS st;

void Task_ReadPos(void *pvPar)
{
	while (1)
	{
		// Serial.println(st.ReadPos(1));
		printf("I'm %s,the servo current pos is %d\r\n", (char *)pvPar, st.ReadPos(1));
		//使用此延时API可以将任务转入阻塞态，期间CPU继续运行其它任务
		vTaskDelay(1 / portTICK_PERIOD_MS);
	}
	vTaskDelete(NULL);
}

void setup()
{
	Serial.begin(115200);
	Serial2.begin(1000000);
	st.pSerial = &Serial2;
	delay(1000);
	st.unLockEprom(1);
	st.WheelMode(1); //恒速模式
	st.LockEprom(1);
	xTaskCreate(
		Task_ReadPos,			  /*任务函数*/
		"Task_ReadPos",			  /*带任务名称的字符串*/
		10000,					  /*堆栈大小，单位为字节*/
		(void *)("Task_ReadPos"), /*作为任务输入传递的参数*/
		1,						  /*任务的优先级*/
		NULL);					  /*任务句柄*/
}

void loop()
{
	st.WriteSpe(1, 3400, 50); //舵机(ID1)以最高速度V=3400步/秒，加速度A=50(50*100步/秒^2)，旋转
	delay(4000);
	st.WriteSpe(1, 0, 50); //舵机(ID1)以加速度A=50(50*100步/秒^2)，停止旋转(V=0)
	delay(2000);
	st.WriteSpe(1, -3400, 50); //舵机(ID1)以最高速度V=-3400步/秒，加速度A=50(50*100步/秒^2)，反向旋转
	delay(4000);
	st.WriteSpe(1, 0, 50); //舵机(ID1)以加速度A=50(50*100步/秒^2)，停止旋转(V=0)
	delay(2000);
	// Serial.println(st.ReadPos(1));
}
